# ===========================================================
# ========================= imports =========================
import time
import numpy as _np
import pandas as pd
from funcs.constants import _CLIGHT, _MAXDTOE, _SYS_BDS, _SYS_GPS, _SYS_UNKNOWN
from funcs.constants import _OMEGAe_GPS,_OMEGAe_BDS
from funcs.constants import _U_GPS,_U_BDS
from funcs.coordinate import xyz2blh
from funcs.time import gpst2Utc, utc2Gpst
# ===========================================================

#------------------------------------------------------------
#--compute satellite clock bias with broadcast ephemeris-----
#------------------------------------------------------------
def eph2clk(t,toc,a0,a1,a2):
    '''
    t            # 观测时间(RINEX——OBS文件   周内秒)
    toc          # 广播星历时间(*.p/*.n/*.rnx文件  周内秒)
    a0           # 卫星钟差(s)
    a1           # 卫星钟速(s/s)
    a2           # 卫星钟速变率(s/s**2)
    '''
    return a0+a1*(t-toc)+a2*_np.power(t-toc,2)

#------------------------------------------------------------
#-------------------get system-------------------------------
#------------------------------------------------------------

def getSystem(PRN):
    '''
    PRN                 # 卫星号
    '''
    if PRN[0]=='G':
        _U=_U_GPS
        _OMEGAe=_OMEGAe_GPS
        sys=_SYS_GPS
    elif PRN[0]=='C':
        _U=_U_BDS
        _OMEGAe=_OMEGAe_BDS
        sys=_SYS_BDS
    else:
        sys=_SYS_UNKNOWN
    return sys


def calSatPos(PRN,roota,delta_n,toe,epoch,m0,e,smallomega,cuc,crc,cic,cus,crs,cis,i0,idot,bigomega0,bigomegadot):
    '''
    计算卫星位置
    roota               # 轨道长半径的平方根
    delta_n             # 由精密星历计算得到的卫星平均角速度与按给定参数计算所得的平均角速度之差(rad/s)
    toe                 # 星历表参考历元(s) 周内秒
    epoch               # 注: 2021-11-10 00:00:00 为 toc
    m0                  # 按参考历元toe计算的平近点角(rad)
    e                   #  轨道偏心率
    smallomega          #  近地点角距(rad)
    cus                 # 纬度幅角的正弦调和项改正的振幅(rad)
    cuc                 # 纬度幅角的余弦调和项改正的振幅(rad)
    crs                 # 轨道半径的正弦调和项改正的振幅(m)
    crc                 # 轨道半径的余弦调和项改正的振幅(m)
    cis                 # 轨道倾角的正弦调和项改正的振幅(rad)
    cic                 # 轨道倾角的余弦调和项改正的振幅(rad)
    i0                  # 按参考历元toe计算的轨道倾角(rad)
    idot                # 轨道倾角变化率(rad/s)
    bigomega0           # 按参考历元toe计算的升交点赤经(rad)
    bigomegadot         # 升交点赤经变化率(rad/s)
    '''

    sys=getSystem(PRN)
    if sys==_SYS_BDS:
        _U=_U_BDS
        _OMEGAe=_OMEGAe_BDS
    else:
        _U=_U_GPS
        _OMEGAe=_OMEGAe_GPS
    

    # 1. 计算卫星运行的平均角速度

    n0 = _np.sqrt(_U)/_np.power(roota,3)
    n  = n0+delta_n

    # 2. 计算归化时间  注:观测时刻为观测文件中伪距或载波相位的接收时刻，因程序不读取观测文件，故暂定观测时刻为toc，即读取的年月日时分秒
    # 将广播星历中GPS时间(年月日时分秒，即toc)转换为GPS周和周内秒形式，此时跳秒为零

    gpst_epoch=utc2Gpst(epoch,leapseconds=0)
    tk=gpst_epoch.second-toe

    if tk>302400:
        tk-=604800
    if tk<-302400:
        tk+=604800

    # 3. 观测时刻卫星平近点角mk的计算

    mk=m0+n*tk

    # 4. 计算偏近点角ek

    ek=mk
    while 1:
        ek_=mk+e*_np.sin(ek)
        if _np.abs(ek-ek_)<10e-12:
            ek=ek_
            break
        else:
            ek=ek_
    if ek<0:
        ek+=2*_np.pi
    
    # 5. 真近点角vk的计算
    
    vk=_np.arctan2(_np.sqrt(1-e**2)*_np.sin(ek),_np.cos(ek)-e)
    # 注:为了在求反三角函数时易于判断真近点角所在的象限，采用半角公式，测试不通过。
    #vk=2*_np.arctan(_np.sqrt((1+e)/(1-e))*_np.arctan2(ek,2))

    # 6. 升交距角

    bigphik=vk+smallomega

    # 7. 摄动改正项的计算

    delta_u=cuc*_np.cos(2*bigphik)+cus*_np.sin(2*bigphik)
    delta_r=crc*_np.cos(2*bigphik)+crs*_np.sin(2*bigphik)
    delta_i=cic*_np.cos(2*bigphik)+cis*_np.sin(2*bigphik)

    # 8. 计算经过摄动改正的升交距角、卫星矢径、轨道倾角

    uk=bigphik+delta_u
    rk=roota**2*(1-e*_np.cos(ek))+delta_r
    ik=i0+delta_i+idot*tk

    # 9. 计算卫星在轨道平面坐标系的坐标

    xk=rk*_np.cos(uk)
    yk=rk*_np.sin(uk)

    if PRN in ['C01','C02','C03','C04','C05','C59','C60','C61','C62']:  # GEO
        # 10. 观测时刻升交点经度

        bigomegak=bigomega0+bigomegadot*tk-_OMEGAe*toe

        # 11. 计算GEO卫星在自定义坐标系中的空间直角坐标

        Xg=xk*_np.cos(bigomegak)-yk*_np.cos(ik)*_np.sin(bigomegak)
        Yg=xk*_np.sin(bigomegak)+yk*_np.cos(ik)*_np.cos(bigomegak)
        Zg=yk*_np.sin(ik)

        # 12. 计算GEO卫星在CGCS2000地固坐标系下的空间直角坐标

        cos5=_np.cos(_np.deg2rad(-5))
        sin5=_np.sin(_np.deg2rad(-5))
        Xk=Xg*_np.cos(_OMEGAe*tk)+Yg*_np.sin(_OMEGAe*tk)*cos5+Zg*_np.sin(_OMEGAe*tk)*sin5
        Yk=-Xg*_np.sin(_OMEGAe*tk)+Yg*_np.cos(_OMEGAe*tk)*cos5+Zg*_np.cos(_OMEGAe*tk)*sin5
        Zk=-Yg*sin5+Zg*cos5
    
    else:
        # 10. 观测时刻升交点经度

        bigomegak=bigomega0+(bigomegadot-_OMEGAe)*tk-_OMEGAe*toe

        # 11. 计算卫星在地心地固坐标系下的位置(WGS-84或CGCS2000)

        Xk=xk*_np.cos(bigomegak)-yk*_np.cos(ik)*_np.sin(bigomegak)
        Yk=xk*_np.sin(bigomegak)+yk*_np.cos(ik)*_np.cos(bigomegak)
        Zk=yk*_np.sin(ik)


    return Xk,Yk,Zk

#--------------------------------------------
#--------------------------------------------
#--------批量处理一个卫星星历数据--------------
#--------------------------------------------
#--------------------------------------------

def calSatPos_Batch(ephemeris,PRN):
    '''
    ephemeris           # 某一GPS卫星星历dataframe
    PRN                 # 卫星号
    '''
    sys=getSystem(PRN)
    satPos=[]
    epochs=[]
    for i in range(0,len(ephemeris)):
        dataline=ephemeris.iloc[i]
        X,Y,Z=calSatPos(PRN,dataline.roota,dataline.delta_n,dataline.toe,dataline.epoch,dataline.m0,
                        dataline.eccentricity,dataline.smallomega,
                        dataline.cuc,dataline.crc,dataline.cic,dataline.cus,dataline.crs,dataline.cis,
                        dataline.i0,dataline.idot,dataline.bigomega0,dataline.bigomegadot)
        [B,L,H]=xyz2blh([X,Y,Z],sys)
        satPos.append([PRN,dataline.epoch,X,Y,Z,_np.rad2deg(B),_np.rad2deg(L),H])
        epochs.append(dataline.epoch)
    
    pos=pd.DataFrame(satPos,columns=['PRN','epoch','X','Y','Z','B','L','H'],index=epochs)
    pos.index.name='Epoch'
    return pos

#--------------------------------------------
#--------------------------------------------
#--------根据观测文件时间序列计算卫星星历序列---
#--------------------------------------------
#--------------------------------------------
def calSatPos_Series(ephemeris,PRN):
    '''
    ephemeris           # 某一GPS卫星星历dataframe
    PRN                 # 卫星号
    '''

    start = time.time()

    sys=getSystem(PRN)
    tdiff_max=_MAXDTOE
    epochs=ephemeris['epoch']

    # 以30s为观测时间间隔，生成30s的时间序列

    satPos=[]
    epoch_min=ephemeris['epoch'][0]
    dayEpochs=pd.date_range(start=epoch_min,periods=2880,freq='30s')

    for i in range(0,len(dayEpochs)):
        # 根据观测时间选择最邻近星历

        tdiff_min=tdiff_max+1
        for j in range(0,len(epochs)):
            tdiff=_np.abs(dayEpochs[i]-epochs[j])
            if tdiff.seconds<tdiff_min:
                tdiff_min=tdiff.seconds
                ephemeris_=ephemeris.iloc[j]
        
        # 对观测时刻作卫星钟差改正

        obsTime=utc2Gpst(dayEpochs[i],leapseconds=0) # 观测时刻
        toc=utc2Gpst(epochs[j],leapseconds=0)        # 星历时刻
        delta_t=eph2clk(obsTime.second,toc.second,float(ephemeris_['clockBias']),float(ephemeris_['relFeqBias']),float(ephemeris_['transmissionTime']))
        obsTimeSecond=obsTime.second-delta_t
        obsTimeUtc=gpst2Utc(obsTime.week,obsTimeSecond,leapseconds=0)

        # 计算卫星位置
        X,Y,Z=calSatPos(PRN,ephemeris_.roota,ephemeris_.delta_n,ephemeris_.toe,obsTimeUtc,ephemeris_.m0,ephemeris_.eccentricity,ephemeris_.smallomega,
                        ephemeris_.cuc,ephemeris_.crc,ephemeris_.cic,ephemeris_.cus,ephemeris_.crs,ephemeris_.cis,ephemeris_.i0,ephemeris_.idot,
                        ephemeris_.bigomega0,ephemeris_.bigomegadot)
        [B,L,H]=xyz2blh([X,Y,Z],sys)
        satPos.append([dayEpochs[i],PRN,X,Y,Z,_np.rad2deg(B),_np.rad2deg(L),H])
    
    pos=pd.DataFrame(satPos,columns=['epoch','PRN','X','Y','Z','B','L','H'],index=dayEpochs)
    pos.index.name='Epoch'

    finish = time.time()     # Time of finish

    print("Calculating satellite position of ", PRN," is cost in", "{0:.2f}".format(finish-start), "seconds.")

    return pos

